/**
 * @file set_area_param.cpp
 * @author fwy (2643660853@qq.com)
 * @brief
 * @version 0.1
 * @date 2025-03-28
 *
 * @copyright Copyright (c) 2025
 *
 */
#include <memory>
#include "std_msgs/msg/int16.hpp"
#include "fishbot_navigation_behaviors/set_area_param.hpp"

static const rclcpp::Logger LOGGER = rclcpp::get_logger("Behavior: SetInitialPose");

namespace fishbot::behaviors
{
    SetAreaParam::SetAreaParam(const std::string &name, const BT::NodeConfig &config,
                               const std::shared_ptr<fishbot::behavior_tree::BehaviorContext> &shared_resources)
        : fishbot::behavior_tree::AsyncBehaviorBase(name, config, shared_resources)
    {
        parameters_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
            this->shared_resources_->node_, "/amcl");
        RCLCPP_INFO_STREAM(LOGGER, "Init");
    }

    BT::PortsList SetAreaParam::providedPorts()
    {
        return BT::PortsList({
            BT::InputPort<std::string>("topic_name"),
            BT::OutputPort<bool>("success"),
        });
    }

    void SetAreaParam::topicCallBack(const std_msgs::msg::Int16::SharedPtr msg)
    {
        int gray_state = msg->data;

        if (gray_state == 0)
        {
            auto future = parameters_client_->set_parameters({rclcpp::Parameter("kld_error", 0.01)});
            RCLCPP_INFO_STREAM(rclcpp::get_logger("SetAreaParam"), ": amcl_setparam_start "<<0.01);
            // 处理回调
            // future.wait();
        }
        else if (gray_state == 1)
        {
            auto future = parameters_client_->set_parameters({rclcpp::Parameter("kld_error", 0.05)});
            RCLCPP_INFO_STREAM(rclcpp::get_logger("SetAreaParam"), ": amcl_setparam_start "<< 0.05);
            // 处理回调
            // future.wait();
        }
        else
        {
            auto future = parameters_client_->set_parameters({rclcpp::Parameter("kld_error", 0.08)});
            RCLCPP_INFO_STREAM(rclcpp::get_logger("SetAreaParam"), ": amcl_setparam_start "<< 0.08);
            // 处理回调
            // future.wait();
        }
    }

    tl::expected<bool, std::string> SetAreaParam::doWork()
    {
        // 获取并检查输入
        std::string topic_name;
        const auto maybe_topic_name = getInput<std::string>("topic_name");
        if (const auto error = fishbot::behavior_tree::maybe_error(maybe_topic_name))
        {
            RCLCPP_ERROR_STREAM(LOGGER, "topic_name maybe error, use default value: /global_costmap/gray_state");
            topic_name = "/global_costmap/gray_state";
        }
        else
        {
            topic_name = maybe_topic_name.value();
        }
        auto node = this->shared_resources_->node_;
        gray_state_sub_ = node->create_subscription<std_msgs::msg::Int16>(
            topic_name, 10, std::bind(&SetAreaParam::topicCallBack, this, std::placeholders::_1));

        setOutput<bool>("success", true);
        // rclcpp::spin(node);
        return true;
    }

}
